#include <main.h>
#include <basic.hpp>
#include <uart.h>
#include <simple_json.h>
#include <mpu.h>
#include <math.h>

#include <app/app_motion.hpp>

using namespace selfHandler;

typedef enum
{
    SYSTEM_STATUS_ERROR = -1,
    SYSTEM_STATUS_IDLE = 0,
    SYSTEM_STATUS_START,
    SYSTEM_STATUS_RUNNING,
} system_status_t;

constexpr float _PI = 3.14159265358979323846;
constexpr float _PITCH_REVISION = -0.058;

pidController *angle_pid = nullptr, *gyro_pid = nullptr, *vol_pid = nullptr;
Motor *motorL = nullptr, *motorR = nullptr;
float vol_ref = 0;

float turn_p = 0;
float turn_ref = 0;
float adjust = 0;

float motor_omega_ref = 0;
bool is_print = false;

system_status_t cur_status = SYSTEM_STATUS_IDLE;

float up_pd_p = -100, up_pd_d = 10, up_pd_m = 100;

float upright_ring_pd(float pitch_ref, float cur_pitch, float gyro)
{
    float output = up_pd_p * (pitch_ref - cur_pitch) + up_pd_d * gyro;
    if (fabs(output) > up_pd_m)
    {
        output = up_pd_m * _sign(output);
    }
    return output;
}

extern "C" void app_main()
{

    serial_init(
        [](uint8_t *data, uint16_t data_len, void *user_data)
        {
            using namespace selfHandler;
            motor_omega_ref = 0;
            cout << "Recv : " << (char *)data << " len = " << data_len << endl;

            float p = 0, i = 0, d = 0, m = 0;
            // if (mjson_get_float((char *)data, "ap", &p) != -1 &&
            //     mjson_get_float((char *)data, "ai", &i) != -1 &&
            //     mjson_get_float((char *)data, "ad", &d) != -1 &&
            //     mjson_get_float((char *)data, "am", &m) != -1)
            // {
            //     angle_pid->reset(p, i, d, m);
            //     cout << "ap = " << p << " ai = " << i << " ad = " << d << " am = " << m << endl;
            // }

            // if (mjson_get_float((char *)data, "gp", &p) != -1 &&
            //     mjson_get_float((char *)data, "gi", &i) != -1 &&
            //     mjson_get_float((char *)data, "gd", &d) != -1 &&
            //     mjson_get_float((char *)data, "gm", &m) != -1)
            // {
            //     gyro_pid->reset(p, i, d, m);
            //     cout << "gp = " << p << " gi = " << i << " gd = " << d << " gm = " << m << endl;
            // }
            if (mjson_get_float((char *)data, "vp", &p) != -1 &&
                mjson_get_float((char *)data, "vi", &i) != -1 &&
                mjson_get_float((char *)data, "vd", &d) != -1 &&
                mjson_get_float((char *)data, "vm", &m) != -1)
            {
                vol_pid->reset(p, i, d, m);
                cout << "vp = " << p << " vi = " << i << " vd = " << d << " vm = " << m << endl;
            }

            if (mjson_get_float((char *)data, "up", &p) != -1 &&
                mjson_get_float((char *)data, "ud", &d) != -1 &&
                mjson_get_float((char *)data, "um", &m) != -1)
            {
                up_pd_p = p;
                up_pd_d = d;
                up_pd_m = m;
                cout << "up = " << p << " ud = " << d << " um = " << m << endl;
            }

            if (mjson_get_float((char *)data, "vol_ref", &vol_ref) != -1)
            {
                cout << "vol_ref = " << vol_ref << " rad/s" << endl;
            }
            if (mjson_get_float((char *)data, "adjust", &adjust) != -1)
            {
                cout << "adjust = " << adjust << endl;
            }

            if (mjson_get_int((char *)data, "status", (int *)&cur_status) != -1)
            {
                cout << "status = " << (int)cur_status << endl;
            }
            if (mjson_get_float((char *)data, "turn_p", &turn_p) != -1)
            {
                cout << "turn_p = " << turn_p << endl;
            }
            if (mjson_get_float((char *)data, "turn_ref", &turn_ref) != -1)
            {
                cout << "turn_ref = " << turn_ref << "°" << endl;
            }

            if (mjson_get_bool((char *)data, "is_print", &is_print) != -1)
            {
                cout << "is_print = " << is_print << endl;
            }

            return;
        },
        nullptr);

    delay(100); /*wait the peripheral device to be ready*/

    mpu_start(mpu_accel_fs_t::MPU_2G, mpu_gyro_fs_t::MPU_250DPS);
    mpu_euler_angle_t angle;
    mpu_acce_gyro_data_t acce_gyro_data;

    extern TIM_HandleTypeDef htim3;
    motorL = new Motor(
        {&htim3, TIM_CHANNEL_3},
        {MOTOR_L_A_GPIO_Port, MOTOR_L_A_Pin},
        {MOTOR_L_B_GPIO_Port, MOTOR_L_B_Pin},
        {ENC_L_A_GPIO_Port, ENC_L_A_Pin},
        {ENC_L_B_GPIO_Port, ENC_L_B_Pin},
        ENC_L_A_EXTI_IRQn);
    motorR = new Motor(
        {&htim3, TIM_CHANNEL_4},
        {MOTOR_R_A_GPIO_Port, MOTOR_R_A_Pin},
        {MOTOR_R_B_GPIO_Port, MOTOR_R_B_Pin},
        {ENC_R_A_GPIO_Port, ENC_R_A_Pin},
        {ENC_R_B_GPIO_Port, ENC_R_B_Pin},
        ENC_R_A_EXTI_IRQn, true);

    angle_pid = new pidController(2, 0, 0, 5);
    gyro_pid = new pidController(50, 0, 0, 100);
    vol_pid = new pidController(-0.09, 0, 0.01, 5);

    selfHandler::cout << "=========Welcome to use the balancing car========" << selfHandler::endl;

    for (;;)
    {
        /*进行一下左右轮子速度修正*/
        // selfHandler::period_executor(
        //     "speed calibration", 10,
        //     [&acce_gyro_data, &angle](uint32_t dt_ms)
        //     {
        //         static float speed = 0;
        //         static float delta = 1;
        //         if (speed >= DETECT_MAX_ANGLE_VOLOCITY)
        //             delta = -1;
        //         else if (speed <= -DETECT_MAX_ANGLE_VOLOCITY)
        //             delta = 1;

        //         speed += delta;

        //         motorL->set_target_angle_volocity(speed);
        //         motorR->set_target_angle_volocity(speed);

        //         cout.set_precision(6)
        //             << "target speed = " << speed << ','
        //             << "r=" << motorR->get_angle_volocity() << ','
        //             << "l=" << motorL->get_angle_volocity() << endl;
        //     });

        // selfHandler::period_executor(
        //     "motor_poll", 10,
        //     [](uint32_t dt_ms)
        //     {
        //         motorL->poll(dt_ms / 1000.0f);
        //         motorR->poll(dt_ms / 1000.0f);
        //         return;
        //     });

        // continue;

        selfHandler::period_executor(
            "main_task", 10,
            [&angle, &acce_gyro_data](uint32_t dt_ms)
            {
                if (dt_ms > 10)
                {
                    cout << "-----WARING: dt_ms = " << dt_ms << endl;
                }

                static int8_t last_sign = 0;
                float dt = dt_ms / 1000.0f;
                float cur_pitch = angle.pitch * _PI / 180.0f;      // rad
                float cur_gyro = acce_gyro_data.gy * _PI / 180.0f; // rad/s

                float cur_vol = motorL->get_angle_volocity() * 0.5 + motorR->get_angle_volocity() * 0.5;
                cur_vol += adjust * cur_gyro;
                float angle_ref = vol_pid->update(vol_ref, cur_vol, dt) + _PITCH_REVISION;
                float output = upright_ring_pd(angle_ref, cur_pitch, cur_gyro);

                switch (cur_status)
                {
                case SYSTEM_STATUS_START:
                {
                    static uint8_t protect_cnt = 0;
                    /*protect 1 : 当角度和角速度方向不一致时, 反转输出,防止由于传感器反应慢导致小车倒向另一边的时候，点击仍然向一边驱动*/
                    if (output * cur_pitch < 0)
                    {
                        output *= -1;
                    }
                    /*protect 2 ：当电机已经朝某个方向持续输出一段时间，但是小车检测 pitch角的符号却始终没有变化，认为此时轮胎抓地力不够，小车无法站立，为防止小车撞损，停止电机*/

                    if (last_sign == _sign(cur_pitch))
                    {
                        if (protect_cnt++ > 75)
                        {
                            cur_status = SYSTEM_STATUS_ERROR;
                            cout << "cur_status = SYSTEM_STATUS_ERROR" << endl;
                            protect_cnt = 0;
                            break;
                        }
                        cout << "protect_cnt = " << protect_cnt << endl;
                    }
                    else
                    {
                        cout << "CHANGE " << _sign(cur_pitch) << " " << last_sign << endl;
                        last_sign = _sign(cur_pitch);
                        protect_cnt = 0;
                    }

                    motorL->set_torque(output);
                    motorR->set_torque(output);

                    if (is_print)
                    {
                        cout.set_precision(5) << "cur_pitch = " << cur_pitch
                                              << ", output = " << output << endl;
                    }

                    /*进入Running状态 : 当角度小于5°，并且持续一段时间后 认为小车已经平衡, 进入Running状态*/
                    if (fabsf(cur_pitch) < 5 * _PI / 180.0f)
                    {
                        static uint8_t cnt = 0;
                        cout << "enter cnt = " << cnt << endl;
                        if (cnt++ > 25)
                        {
                            cur_status = SYSTEM_STATUS_RUNNING;
                            cout << "==============ENTER SYSTEM_STATUS_RUNNING================" << endl;
                            cnt = 0;
                        }
                    }
                }
                break;
                case SYSTEM_STATUS_RUNNING:
                {
                    /*protect 3 : 当小车意外倒地时，停止运动*/
                    if (fabsf(angle.pitch) > 29 && cur_status == SYSTEM_STATUS_RUNNING)
                    {
                        cur_status = SYSTEM_STATUS_ERROR;
                        motorL->set_torque(0);
                        motorR->set_torque(0);
                        cout << "The car is fallen, stop the motion." << endl;
                        cout << "angle.pitch = " << angle.pitch << endl;
                    }
                    // /*拿起检测*/
                    // if (fabsf(output) > DETECT_MAX_ANGLE_VOLOCITY - 10) /*high speed*/
                    // {
                    //     float cur_acce = sqrtf(powf(acce_gyro_data.ax, 2) + powf(acce_gyro_data.ay, 2) + powf(acce_gyro_data.az, 2));
                    //     cout << "cur_acce = " << cur_acce << endl;

                    //     static uint8_t cnt = 0;
                    //     if (cur_acce < 1.5) // 加速度小于1.5g,认为是被拿起 轮子在空转
                    //     {

                    //         cout << "cur_acce = " << cur_acce << endl;
                    //         cout << "cnt = " << cnt << endl;

                    //         if (cnt++ > 30)
                    //         {
                    //             cout << "==============ENTER SYSTEM_STATUS_IDLE================" << endl;

                    //             cur_status = SYSTEM_STATUS_IDLE;
                    //             cnt = 0;
                    //         }
                    //     }
                    //     else if (cnt > 0)
                    //         cnt--;
                    // }

                    float l_output = output + turn_p * acce_gyro_data.gz;
                    float r_output = output - turn_p * acce_gyro_data.gz;
                    // l_output *= (1.0f + turn_ref);
                    // r_output *= (1.0f - turn_ref);

                    motorL->set_target_angle_volocity(l_output);
                    motorR->set_target_angle_volocity(r_output);
                    motorL->poll(dt);
                    motorR->poll(dt);

                    if (is_print)
                    {
                        // cout << angle.pitch << endl;
                        cout << "pitch = " << cur_pitch << ','
                             << "gyro = " << cur_gyro << ','
                             << "angle_ref = " << angle_ref << ','
                             << "output = " << output << endl;
                    }
                }
                break;

                default:
                case SYSTEM_STATUS_IDLE:
                {
                    selfHandler::period_executor(
                        "logi_task", 800,
                        [](uint32_t dt_ms)
                        {
                            cout << "cur_status = SYSTEM_STATUS_IDLE" << endl;
                        });
                    motorL->set_torque(0);
                    motorR->set_torque(0);
                }

                break;
                }

                return;
            });

        selfHandler::period_executor(
            "imu_task", 5,
            [&acce_gyro_data, &angle](uint32_t dt_ms)
            {
                auto ret = mpu_loop(&acce_gyro_data, &angle, dt_ms / 1000.0f);
                if (ret < 0)
                {
                    cout << "========mpu_loop error========" << endl;
                }

                return;
            });

        selfHandler::period_executor(
            "led_infos_task", cur_status == SYSTEM_STATUS_RUNNING ? 200 : 1000,
            [](uint32_t dt_ms)
            {
                HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
            });
    }
}
